#ifndef CAN_RECEIVE_H
#define CAN_RECEIVE_H

#include "struct_typedef.h"
#include "bsp_can.h"
#include "main.h"

/*******************************关节电机*******************************/

#define MIT_MODE 0x000
#define POS_MODE 0x100
#define SPEED_MODE 0x200

#define P_MIN -12.5f
#define P_MAX 12.5f
#define V_MIN -20.5f
#define V_MAX 20.5f
#define KP_MIN 0.0f
#define KP_MAX 500.0f
#define KD_MIN 0.0f
#define KD_MAX 5.0f
#define T_MIN -32.0f
#define T_MAX 32.0f

typedef struct 
{
		uint8_t id;
		uint8_t err_state;
		int32_t p_int;
		int32_t v_int;
		int32_t t_int;
		int32_t kp_int;
		int32_t kd_int;
		fp32 pos;
		fp32 vel;
		fp32 tor;
		fp32 Kp;
		fp32 Kd;
		fp32 Temp;
}motor_fbpara_t;

typedef struct
{
		uint16_t mode;
		motor_fbpara_t para;
}Joint_Motor_t ;

extern Joint_Motor_t joint_motor_measure[3];

extern void enable_motor_mode(FDCAN_HandleTypeDef* hcan, uint16_t motor_id, uint16_t mode_id);
extern void disable_motor_mode(FDCAN_HandleTypeDef *hcan, uint16_t motor_id, uint16_t mode_id);
extern fp32 Hex_To_Float(uint32_t *Byte, uint32_t num);
extern uint32_t FloatTohex(fp32 HEX);
extern int32_t float_to_uint(fp32 x_float, fp32 x_min, fp32 x_max, uint32_t bits);
extern fp32 uint_to_float(uint32_t x_int, fp32 x_min, fp32 x_max, uint32_t bits);
extern void joint_motor_init(Joint_Motor_t *motor, uint16_t id, uint16_t mode);
extern void MI_motor_fbdata(Joint_Motor_t *motor, uint8_t *rx_data, uint32_t data_len);
extern void mit_ctrl_joint(FDCAN_HandleTypeDef *hcan, uint16_t motor_id, fp32 pos, fp32 vel, fp32 kp, fp32 kd, fp32 torq);
extern void pos_speed_ctrl(FDCAN_HandleTypeDef *hcan, uint16_t motor_id, fp32 pos, fp32 vel);
extern void speed_ctrl(FDCAN_HandleTypeDef *hcan, uint16_t motor_id, fp32 vel);

/*******************************dji电机*******************************/

#define CHASSIS_CAN	 hfdcan1
#define ARM_CAN		 hfdcan3

typedef enum
{
	CAN_CHASSIS_ALL_ID = 0x200,
	CAN_ARM_ALL_ID = 0x1FF,

	CAN_3508_M1_ID = 0x201,
	CAN_3508_M2_ID = 0x202,
	CAN_3508_M3_ID = 0x203,
	CAN_3508_M4_ID = 0x204,

	CAN_6020_j3_ID = 0x205,
	CAN_2006_j4_ID = 0x206,
	CAN_2006_j5_ID = 0x207,
}
can_msg_id_e;

typedef struct
{
    uint16_t ecd;
    int16_t speed_rpm;
    int16_t given_current;
    uint8_t temperate;
    int16_t last_ecd;
}motor_measure_t;

extern motor_measure_t motor_arm[3];
extern motor_measure_t motor_chassis[4];

extern void CAN_cmd_chassis(int16_t chassis_lf, int16_t cahssis_rf, int16_t chassis_rb, int16_t chassis_lb);
extern void CAN_cmd_arm(int16_t j3_6020, int16_t j4_2006, int16_t j5_2006, int16_t rev);

extern const motor_measure_t *get_chassis_motor_measure_point(uint8_t i);
extern const motor_measure_t *get_arm_j4_motor_measure_point(void);
extern const motor_measure_t *get_arm_j5_motor_measure_point(void);
extern const motor_measure_t *get_arm_j3_motor_measure_point(void);

#endif
